#include <stdio.h>
#include <unistd.h>
#include "ohos_init.h"
#include "ohos_types.h"
#include "cmsis_os2.h"
#include "wifi_connecter.h"
#include "lwip/sockets.h"
#include "lwip/netdb.h"
#include "iot_gpio.h"
#include "iot_gpio_ex.h"
#include "iot_pwm.h"
#include "cJSON.h"

#define GPIO0 0
#define GPIO1 1
#define GPIO9 9
#define GPIO10 10
#define GPIO7 7
#define GPIO8 8
#define PWM_CHANNEL_3 3
#define PWM_CHANNEL_4 4
#define PWM_CHANNEL_0 0
#define PWM_CHANNEL_1 1
#define CAR_STEP_COUNT 20

typedef enum {
    /* 停止 */
    CAR_STATUS_STOP,
    /* 前进 */
    CAR_STATUS_FORWARD,
    /* 后退 */
    CAR_STATUS_BACKWARD,
    /* 左转 */
    CAR_STATUS_LEFT,
    /* 右转 */
    CAR_STATUS_RIGHT,
    /** Maximum value */
    CAR_STATUS_MAX
} CarStatus;

CarStatus curStatus = CAR_STATUS_STOP;
CarStatus nextStatus = CAR_STATUS_STOP;
int stepCount = 0;

void car_stop(void)
{
    IoTGpioSetOutputVal(GPIO0, IOT_GPIO_VALUE1);
    IoTGpioSetOutputVal(GPIO1, IOT_GPIO_VALUE1);
    IoTGpioSetOutputVal(GPIO9, IOT_GPIO_VALUE1);
    IoTGpioSetOutputVal(GPIO10, IOT_GPIO_VALUE1);
}

void car_forward(void)
{
    IoTGpioSetOutputVal(GPIO0, IOT_GPIO_VALUE1);
    IoTGpioSetOutputVal(GPIO1, IOT_GPIO_VALUE0);
    IoTGpioSetOutputVal(GPIO9, IOT_GPIO_VALUE1);
    IoTGpioSetOutputVal(GPIO10, IOT_GPIO_VALUE0);
}

void car_backward(void)
{
    IoTGpioSetOutputVal(GPIO0, IOT_GPIO_VALUE0);
    IoTGpioSetOutputVal(GPIO1, IOT_GPIO_VALUE1);
    IoTGpioSetOutputVal(GPIO9, IOT_GPIO_VALUE0);
    IoTGpioSetOutputVal(GPIO10, IOT_GPIO_VALUE1);
}

void car_left(void)
{
    IoTGpioSetOutputVal(GPIO0, IOT_GPIO_VALUE0);
    IoTGpioSetOutputVal(GPIO1, IOT_GPIO_VALUE0);
    IoTGpioSetOutputVal(GPIO9, IOT_GPIO_VALUE1);
    IoTGpioSetOutputVal(GPIO10, IOT_GPIO_VALUE0);
}

void car_right(void)
{
    IoTGpioSetOutputVal(GPIO0, IOT_GPIO_VALUE1);
    IoTGpioSetOutputVal(GPIO1, IOT_GPIO_VALUE0);
    IoTGpioSetOutputVal(GPIO9, IOT_GPIO_VALUE0);
    IoTGpioSetOutputVal(GPIO10, IOT_GPIO_VALUE0);
}

float GetDistance(void) {
    static unsigned long start_time = 0, time = 0;
    unsigned int flag = 0;
    IotGpioValue value;
    IoTGpioSetOutputVal(GPIO7, IOT_GPIO_VALUE1);
    usleep(20);
    IoTGpioSetOutputVal(GPIO7, IOT_GPIO_VALUE0);
    while (1) {
        IoTGpioGetInputVal(GPIO8, &value);
        if (value == IOT_GPIO_VALUE1 && flag == 0) {
            start_time = hi_get_us();
            flag = 1;
        }
        if (value == IOT_GPIO_VALUE0 && flag == 1) {
            time = hi_get_us() - start_time;
            start_time = 0;
            break;
        }
    }
    return time * 0.034 / 2;
}

static void CarTask(void)
{
    // 初始化 GPIO
    IoTGpioInit(GPIO0);
    IoTGpioSetFunc(GPIO0, IOT_GPIO_FUNC_GPIO_0_GPIO);
    IoTGpioSetDir(GPIO0, IOT_GPIO_DIR_OUT);
    IoTGpioInit(GPIO1);
    IoTGpioSetFunc(GPIO1, IOT_GPIO_FUNC_GPIO_1_GPIO);
    IoTGpioSetDir(GPIO1, IOT_GPIO_DIR_OUT);
    IoTGpioInit(GPIO9);
    IoTGpioSetFunc(GPIO9, IOT_GPIO_FUNC_GPIO_9_GPIO);
    IoTGpioSetDir(GPIO9, IOT_GPIO_DIR_OUT);
    IoTGpioInit(GPIO10);
    IoTGpioSetFunc(GPIO10, IOT_GPIO_FUNC_GPIO_10_GPIO);
    IoTGpioSetDir(GPIO10, IOT_GPIO_DIR_OUT);
    IoTGpioInit(GPIO7);
    IoTGpioSetFunc(GPIO7, IOT_GPIO_FUNC_GPIO_7_GPIO);
    IoTGpioSetDir(GPIO7, IOT_GPIO_DIR_OUT);
    IoTGpioInit(GPIO8);
    IoTGpioSetFunc(GPIO8, IOT_GPIO_FUNC_GPIO_8_GPIO);
    IoTGpioSetDir(GPIO8, IOT_GPIO_DIR_IN);

    car_stop();
    while (1)
    {
        if (nextStatus == CAR_STATUS_FORWARD && GetDistance() < 10.0f)
        {
            nextStatus = CAR_STATUS_STOP;
        }
        if (curStatus != nextStatus)
        {
            printf("Next status: %d\n", nextStatus);
            switch (nextStatus)
            {
                case CAR_STATUS_STOP:
                    car_stop();
                    break;
                case CAR_STATUS_FORWARD:
                    car_forward();
                    break;
                case CAR_STATUS_BACKWARD:
                    car_backward();
                    break;
                case CAR_STATUS_LEFT:
                    car_left();
                    break;
                case CAR_STATUS_RIGHT:
                    car_right();
                    break;
                default:
                    break;
            }
            stepCount = CAR_STEP_COUNT;
            curStatus = nextStatus;
        }
        if (curStatus != CAR_STATUS_STOP)
        {
            stepCount--;
            if (stepCount <= 0)
            {
                nextStatus = CAR_STATUS_STOP;
            }
        }
        usleep(1000);
    }
}

static void UDPTask(void)
{
    int ret = 0;
    // 连接至热点
    WifiDeviceConfig config = {0};
    strcpy_s(config.ssid, sizeof(config.ssid), "YOUR_SSID"); // 填写热点的 SSID
    strcpy_s(config.preSharedKey, sizeof(config.preSharedKey), "PASSWORD"); // 填写热点密码
    config.securityType = WIFI_SEC_TYPE_PSK;
    printf("Connecting wifi...\n");
    int netId = ConnectToHotspot(&config);
    printf("ConnectToHotspot: %d\n", netId);
    // 填充 sockaddr_in 结构体
    struct sockaddr_in server_addr = {0};
    server_addr.sin_family = AF_INET;
    server_addr.sin_addr.s_addr = htonl(INADDR_ANY);
    server_addr.sin_port = htons(3861);
    // 创建 Socket
    int fd = socket(PF_INET, SOCK_DGRAM, 0);
    if (fd < 0)
    {
        printf("Socket allocate failed, errno=%d\n", errno);
        return;
    }
    int opt = 1;
    // 设置该套接字为广播类型
    if (setsockopt(fd, SOL_SOCKET, SO_BROADCAST, (char*) &opt, sizeof(opt)) < 0)
    {
        printf("Set socket broadcast failed\n");
        lwip_close(fd);
        return;
    }
    // 开始监听
    if (bind(fd, (struct sockaddr*) &server_addr, sizeof(server_addr)) != 0)
    {
        printf("Socket bind failed\n");
        lwip_close(fd);
        return;
    }
    printf("Socket listening\n");
    char buffer[1024];
    while (1)
    {
        // 接收消息
        struct sockaddr_in addr;
        int addr_len = sizeof(addr);
        ret = recvfrom(fd, buffer, sizeof(buffer), 0, (struct sockaddr*) &addr, (socklen_t*) &addr_len);
        if (ret < 0)
        {
            printf("recvfrom failed, ret=%d!\n", ret);
            continue;
        }
        buffer[ret] = '\0';
        // 把对方的IP地址打印出来
        printf("%s:%d says: %s\n", inet_ntoa(addr.sin_addr), ntohs(addr.sin_port), buffer);
        // 解析 JSON
        cJSON *json = cJSON_Parse(buffer);
        if (json != NULL)
        {
            if (cJSON_GetObjectItem(json, "cmd")->valuestring != NULL)
            {
                if (strcmp("forward", cJSON_GetObjectItem(json, "cmd")->valuestring) == 0)
                {
                    nextStatus = CAR_STATUS_FORWARD;
                }
                else if (strcmp("backward", cJSON_GetObjectItem(json, "cmd")->valuestring) == 0)
                {
                    nextStatus = CAR_STATUS_BACKWARD;
                }
                else if (strcmp("left", cJSON_GetObjectItem(json, "cmd")->valuestring) == 0)
                {
                    nextStatus = CAR_STATUS_LEFT;
                }
                else if (strcmp("right", cJSON_GetObjectItem(json, "cmd")->valuestring) == 0)
                {
                    nextStatus = CAR_STATUS_RIGHT;
                }
                else if (strcmp("stop", cJSON_GetObjectItem(json, "cmd")->valuestring) == 0)
                {
                    nextStatus = CAR_STATUS_STOP;
                }
            }
            cJSON_Delete(json);
        }
    }
}

void CarExample(void)
{
    osThreadAttr_t attr;
    attr.name = "CarTask";
    attr.attr_bits = 0U;
    attr.cb_mem = NULL;
    attr.cb_size = 0U;
    attr.stack_mem = NULL;
    attr.stack_size = 2048;
    attr.priority = osPriorityNormal;
    if (osThreadNew((osThreadFunc_t) CarTask, NULL, &attr) == NULL)
    {
        printf("Failed to create CarTask!\n");
    }
    attr.name = "UDPTask";
    attr.stack_size = 4096;
    if (osThreadNew((osThreadFunc_t) UDPTask, NULL, &attr) == NULL)
    {
        printf("Failed to create UDPTask!\n");
    }
}

SYS_RUN(CarExample);
